#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Author  : Lee
# @File    : get_localization_points.py
# @Time    : 2023/12/13 11:03

import numpy as np
from util.common_calc import *


class GetLocalizationPoints:
    # 初始化函数需传入A、B点坐标，起始线到A点距离，定位点列表，均分数量
    def __init__(self, base_line_points, start_point_distance, point_list, aver_nums=49):
        self.aver_nums = aver_nums
        self.point_list = point_list
        self.point_a, self.point_b = base_line_points
        self.start_point_distance = start_point_distance

    def calc_max_online_dis(self):
        # 步骤1：计算基准线直线方程
        a, b, c = calc_baseline_equation(self.point_a, self.point_b)

        dis_list = []
        for point in self.point_list:
            dis_list.append(get_distance_of_point_to_line(point, a, b, c))
        return max(dis_list) * 100

    def get_aver_point(self, aver_type, _points, _average_points, a, b, c, is_get_points=False):
        if len(_points) <= len(_average_points):  # 判断筛选点数量是否大于等分数量，若小于则全部返回
            self.point_list = _points
        else:
            # 分别使用等分AB线、等分轨迹线、等分数据的形式计算误差
            if aver_type == "ave_line":
                # 等分AB上轨迹方法
                _line_average_points = [_points[0]]
                _cur_location_index = 1
                for index in range(1, len(_average_points) - 1):  # 起始线、终止线参与计算，不参与遍历
                    # 求最近点
                    _cur_point = _average_points[index]
                    # y = b / a * x + a_z
                    # 遍历等分点，通过等分点作AB线垂线
                    _z = _cur_point[1] - b / a * _cur_point[0]
                    _cur_location_point_to_line_list = list()
                    for _location_index in range(_cur_location_index, len(_points) - 1):
                        # 分别计算轨迹线到上述垂线的距离，求出最小值
                        _cur_location_point_to_line_list.append(
                            get_distance_of_point_to_line(_points[_location_index], -b, a, -a * _z))
                    min_index = np.argmin(_cur_location_point_to_line_list)
                    _cur_location_index = _cur_location_index + min_index
                    _line_average_points.append(_points[_cur_location_index])
                _line_average_points.append(_points[-1])
                self.point_list = _line_average_points
            elif aver_type == "ave_counts":
                # 等分定位点数量
                _line_average_points = [_points[0]]
                _step = len(_points) / self.aver_nums
                for i in range(0, len(_points)):
                    if i > _step * (len(_line_average_points)):
                        if i - _step * (len(_line_average_points)) < i - _step * (len(_line_average_points)) - 1:
                            _line_average_points.append(_points[i])
                        else:
                            _line_average_points.append(_points[i - 1])
                _line_average_points.append(_points[-1])
                self.point_list = _line_average_points
            elif aver_type == "ave_trajectory":
                # 等分轨迹
                _point_distance = []
                _line_average_points = [_points[0]]
                for i in range(0, len(_points) - 1):
                    _point_distance.append(get_distance_of_two_point(_points[i], _points[i + 1]))

                # 轨迹长度
                _trajectory_distance = sum(_point_distance)

                # 平均每段轨迹长度
                _average_trajectory_distance = _trajectory_distance / self.aver_nums

                _sum = 0
                for index in range(len(_point_distance)):
                    _sum = _sum + _point_distance[index]
                    if _sum > _average_trajectory_distance:
                        if _sum - _average_trajectory_distance < _average_trajectory_distance - (
                                _sum - _point_distance[index]):
                            _line_average_points.append(_points[index])
                        else:
                            _line_average_points.append(_points[index - 1])
                        _sum = 0
                _line_average_points.append(_points[-1])
                self.point_list = _line_average_points

        # 衔接行测试只返回均分后的坐标点
        if is_get_points:
            return self.point_list

        dis_list = []
        _speed = 0
        for point in self.point_list:
            _speed = _speed + point[2]
            dis_list.append(get_distance_of_point_to_line(point, a, b, c))
        return max(dis_list), statistics.mean(dis_list), get_variance(dis_list), _speed / len(self.point_list)

    def start_calc(self, test_type, end_line_dis=0):
        # 步骤1：计算基准线直线方程
        a, b, c = calc_baseline_equation(self.point_a, self.point_b)

        # 分别计算出经过A、B两点且垂直于AB直线的方程
        # 经过A点且垂直于AB直线的方程为： y = b / a * x + a_z
        # 经过B点且垂直于AB直线的方程为： y = b / a * x + B_z
        a_z = self.point_a[1] - b / a * self.point_a[0]
        b_z = self.point_b[1] - b / a * self.point_b[0]

        _points = list()
        # if test_type == "ab_line":
        #     # AB线测试，筛选出在经过A、B点且垂直于AB直线的两条平行线之间的点（点在一条直线左侧、另一条右侧）
        #     # 从定位点中筛选出符合要求的点集
        if test_type == "ab_line":
            end_line_dis = self.start_point_distance

        for point in self.point_list:
            _point_location = [get_point_location_of_line(point, -b, a, -a * a_z),
                               get_point_location_of_line(point, -b, a, -a * b_z)]
            if "up" in _point_location and "down" in _point_location:
                # 若点在两条平行线之间，获取起始点和终止点（判断条件为，起止点到A距离大于阈值，终止点到B距离大于阈值）
                _dis_start_to_a = get_distance_of_two_point(self.point_a, point)
                _dis_end_to_b = get_distance_of_two_point(self.point_b, point)
                if _dis_start_to_a >= self.start_point_distance and _dis_end_to_b >= end_line_dis:
                    _points.append(point)
        # elif test_type == "a_plus_line":
        #     _points = self.point_list

        # 获取起始线和终止线在AB线上的投影坐标，并将投影间距离平均分成49份
        _a = get_intersection(self.point_a, self.point_b, _points[0])
        _b = get_intersection(self.point_a, self.point_b, _points[-1])
        _average_points = ior_calc_average_divide_line(_a, _b, self.aver_nums)

        ave_line_res = self.get_aver_point("ave_line", _points, _average_points, a, b, c)
        ave_counts_res = self.get_aver_point("ave_counts", _points, _average_points, a, b, c)
        ave_trajectory_res = self.get_aver_point("ave_trajectory", _points, _average_points, a, b, c)

        return {
            "均分轨迹点": {
                "最大误差(cm)": ave_counts_res[0] * 100,
                "平均误差(cm)": ave_counts_res[1] * 100,
                "直线精度(cm)": ave_counts_res[2] * 100,
                "平均速度(km/h)": ave_counts_res[3] * 3.6
            },
            "均分轨迹长度": {
                "最大误差(cm)": ave_trajectory_res[0] * 100,
                "平均误差(cm)": ave_trajectory_res[1] * 100,
                "直线精度(cm)": ave_trajectory_res[2] * 100,
                "平均速度(km/h)": ave_trajectory_res[3] * 3.6
            },
            "均分轨迹在基准线上的投影": {
                "最大误差(cm)": ave_line_res[0] * 100,
                "平均误差(cm)": ave_line_res[1] * 100,
                "直线精度(cm)": ave_line_res[2] * 100,
                "平均速度(km/h)": ave_line_res[3] * 3.6
            }
        }
